/*
 * Copyright 2010-2012 Marcin Blazejewski
 *
 * This file is part of Ground0.
 *
 * Ground0 is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * Ground0 is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with Ground0. If not, see <http://www.gnu.org/licenses/>.
 *
 */

#include <config.h>

#include "stuff.hpp"
#include "eye.hpp"
#include "geometry.hpp"
#include "collision.hpp"

#include <GL/gl.h>
#include <GL/glu.h>
#ifdef WIN32
#include <GL/glext.h>
#endif
#include <SDL/SDL.h>
#include <string.h>
#include <math.h>
#include <iostream>

using namespace std;



/******************************************************/


Stuff::Stuff(Block *b) : block(b), front(0,0,1), up(0,1,0) {
    cR = 0;
    cidx = -1;
    scale = 1;
    use_grav = false;
    rigid = false;
}

Stuff::Stuff(const Stuff &r) : block(r.block) {
    front = r.front;
    up = r.up;
    velocity = r.velocity;
    pos = r.pos;
    orientation = r.orientation;
    cR = r.cR;
    scale = r.scale;
    cm = r.cm;
    
    cidx = -1;    
    use_grav = r.use_grav;
    rigid = r.rigid;

    selbox_list = 0;
}

Stuff::~Stuff() {
}

void Stuff::move(float x, float y, float z) {
    prevpos = pos;
    pos.x += x;
    pos.y += y;
    pos.z += z;
}

void Stuff::move(const Vector &dp) {
    pos += dp;
}

void Stuff::draw() {
    static GLfloat m[16];

    glTranslatef(pos.x, pos.y, pos.z);
    glScalef(scale, scale, scale);
    orientation.get_matrix(m);
    glMultMatrixf(m);
    block->draw();
}

void Stuff::draw_sel() {
    if (selbox_list == 0) prepare();
    static GLfloat m[16];
    
    glTranslatef(pos.x, pos.y, pos.z);
    glScalef(scale, scale, scale);
    orientation.get_matrix(m);
    glMultMatrixf(m);

    glCallList(selbox_list);
}

#define min(a,b)  ( (a) < (b) ? (a) : (b) )
#define max(a,b)  ( (a) > (b) ? (a) : (b) )

void Stuff::prepare() {
    selbox_list = glGenLists(1);

    xl = yl = zl = xh = yh = zh = 0;
    for (int i = 0; i < block->vertices.size(); i++) {
        xl = min(xl, block->vertices[i]->x);
        yl = min(yl, block->vertices[i]->y);
        zl = min(zl, block->vertices[i]->z);
        xh = max(xh, block->vertices[i]->x);
        yh = max(yh, block->vertices[i]->y);
        zh = max(zh, block->vertices[i]->z);
    }


    glNewList(selbox_list, GL_COMPILE);
    glBegin(GL_LINES);

    // top
    glVertex3f(xl, yh, zl); glVertex3f(xl, yh, zh);
    glVertex3f(xl, yh, zh); glVertex3f(xh, yh, zh);
    glVertex3f(xh, yh, zh); glVertex3f(xh, yh, zl);
    glVertex3f(xh, yh, zl); glVertex3f(xl, yh, zl);
    // bottom
    glVertex3f(xl, yl, zl); glVertex3f(xl, yl, zh);
    glVertex3f(xl, yl, zh); glVertex3f(xh, yl, zh);
    glVertex3f(xh, yl, zh); glVertex3f(xh, yl, zl);
    glVertex3f(xh, yl, zl); glVertex3f(xl, yl, zl);
    // front
    glVertex3f(xl, yl, zh); glVertex3f(xl, yh, zh);
    glVertex3f(xl, yh, zh); glVertex3f(xh, yh, zh);
    glVertex3f(xh, yh, zh); glVertex3f(xh, yl, zh);
    glVertex3f(xh, yl, zh); glVertex3f(xl, yl, zh);
    // back
    glVertex3f(xl, yl, zl); glVertex3f(xl, yh, zl);
    glVertex3f(xl, yh, zl); glVertex3f(xh, yh, zl);
    glVertex3f(xh, yh, zl); glVertex3f(xh, yl, zl);
    glVertex3f(xh, yl, zl); glVertex3f(xl, yl, zl);
    

    glEnd();
    glEndList();
}

void Stuff::tick(const float dt) {
    prev_o = orientation;
    prevpos = pos;

    pos += velocity * dt;

    // TODO: optimize: remove double lenght calc
    _rot_axis = rotvel.get_normal();
    float w = rotvel.length();

    Quaternion dO(dt * w, _rot_axis);
    orientation = dO * orientation;
    orientation.normalize();

    front = Vector::z_axis * orientation;
    up = Vector::y_axis * orientation;
    cm->invalidate();
}

void Stuff::calc_cr() {
    cR = 0;
    for (int i = 0; i < block->vertices.size(); i++) {
        if (cR < block->vertices[i]->length()) cR = block->vertices[i]->length();
    }
}

bool Stuff::collides_with(Stuff *s, Vector *n, Vector *p1, Vector *p2) {
    return cm->test(*this, *s, n, p1, p2);
}


/******************************************************************************/
/******************************************************************************/

Dynamics::Dynamics() {
    for (int i = 0; i < 3; i++) {
        memset(I[i], 0, sizeof(float) * 3);
        memset(I_inv[i], 0, sizeof(float) * 3);
    }
    mass = 0;
}

void Dynamics::add_point_mass(Vector center, float mass) {
    I[0][0] += mass * (center.y * center.y + center.z * center.z);
    I[0][1] += mass * -1 * center.x * center.y;
    I[0][2] += mass * -1 * center.x * center.z;
    
    I[1][0] += mass * -1 * center.x * center.y;
    I[1][1] += mass * (center.x * center.x + center.z * center.z);
    I[1][2] += mass * -1 * center.y * center.z;
    
    I[2][0] += mass * -1 * center.x * center.z;
    I[2][1] += mass * -1 * center.y * center.z;
    I[2][2] += mass * (center.x * center.x + center.y * center.y);
}

void Dynamics::from_cm(CollisionModel &cm) {
    list<CollisionNode*>::iterator i;
    float r_sum = 0.0;
    float m_sum = 0.0;
    static int T_STEPS = 20;
    static int A_STEPS = 100;
    static int R_STEPS = 100;
    int ai, ti, ri;
    float a, t, r;
    float x, y, z;

    for (i = cm.nodes.begin(); i != cm.nodes.end(); i++) {
        CollisionSphere *cs = (CollisionSphere*)*i;
        float r = cs->radius;
        
        for (x = -r; x <= r; x += (2*r/R_STEPS)) {
            for (y = -r; y <= r; y += (2*r/R_STEPS)) {
                for (z = -r; z <= r; z += (2*r/R_STEPS)) {
                    Vector v(x, y, z);
                    if (v.length() > r) continue;
                    
                    add_point_mass(cs->center + v, 1);
                    m_sum += 1;
                }
            }
        }
        
    }

    float f = mass / m_sum;
    for (int p = 0; p < 3; p++) {
        for (int q = 0; q < 3; q++) {
            I[p][q] *= f;
        }
    }

    mat_inverse_3x3(I, I_inv);
}

void Dynamics::collide(Stuff &s1, Stuff &s2, Vector &n, Vector &p1, Vector &p2, float T) {
    static float e = .9;  // coefficient of restitution 0-1

    //
    // PREPARE INPUTS
    //

    float m1 = mass;
    float m2 = s2.dyn.mass;

    // collision points and penetration vector
    Vector r1 = p1 - s1.pos;
    Vector r2 = p2 - s2.pos;
    Vector r21 = p2 - p1;

    // velocity of bodies at contact point
    Vector vp1 = s1.velocity + s1.rotvel.cross(r1);
    Vector vp2 = s2.velocity + s2.rotvel.cross(r2);
    // relative velocity at contact point
    Vector vr = vp2 - vp1;

    //
    // CALCULATE REACTION IMPULSE MAGNITUDE
    //
    
    Vector _up = vr * (-1-e);
    Vector _tmp1 = r1.cross(n).left_multiply(s1.dyn.I_inv).cross(r1);
    Vector _tmp2 = r2.cross(n).left_multiply(s2.dyn.I_inv).cross(r2);
    Vector _down = _tmp1 + _tmp2;
    float j = _up.dot(n) / ( 1.0/m1 + 1.0/m2 + _down.dot(n) );
    j = fabs(j);


    //
    // USE THE IMPULSE TO AFFECT THE BODIES
    //

    Vector _dv1 = ( n * (-j / m1) );  // linear velocity change for s1
    Vector _dv2 = ( n * ( j / m2) );  // linear velocity change for s2
    Vector _dw1 = (r1.cross(n).left_multiply(s1.dyn.I_inv) * -j);  // rot vel chg for s1
    Vector _dw2 = (r2.cross(n).left_multiply(s2.dyn.I_inv) *  j);  // rot vel chg for s2

    s1.velocity += _dv1;
    s2.velocity += _dv2;
    s1.rotvel += _dw1;
    s2.rotvel += _dw2;
}




